#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
六足机器人高级控制接口
Hexapod Robot Advanced Control Interface

提供统一的动作接口，支持可配置的全局参数和校准数据
Provides unified action interface with configurable global parameters and calibration data

作者: YAHBOOM | Gentle Xu
Author: YAHBOOM | Gentle Xu
日期: 2025/09
Date: 2025/09
"""

import time
import random
import subprocess
import os
import json
from typing import Optional, Union, Dict, Any
from core.MutoLib import Muto
from Largemodel import (
    ConfigManager, 
    RobotController, 
    SensorManager, 
    PositionManager, 
    ResponseValidator, 
    NavigationManager,
    ActionLearningManager,
    ensure_response_format
)


class MutoController:
    """
    六足机器人高级控制器
    Advanced controller for hexapod robot
    
    提供统一的动作接口，支持多种参数组合和优先级处理
    Provides unified action interface with multiple parameter combinations and priority handling
    """
    
    def __init__(self, port: str = "/dev/myserial", debug: bool = True, 
                 calibration_distance_cm: float = 28, calibration_steps: int = 10):
        """
        初始化机器人控制器
        Initialize robot controller
        
        Args:
            port: 串口端口 Serial port
            debug: 调试模式 Debug mode
            calibration_distance_cm: 校准距离(厘米) Calibration distance in cm
            calibration_steps: 校准步数 Calibration steps
        """
        self.debug = debug  # 保存调试模式设置
        self.hardware_connected = False
        
        # 初始化硬件连接
        self.robot = Muto(port=port, debug=debug)
        self.hardware_connected = True
        
        # 初始化各个功能模块
        self.config_manager = ConfigManager(calibration_distance_cm, calibration_steps, debug)
        self.robot_controller = RobotController(self.robot, self.config_manager.default_config, self.config_manager.step_distance_cm, self.config_manager.speed_calibration)
        self.sensor_manager = SensorManager(self.robot)
        self.position_manager = PositionManager(self)
        self.navigation_manager = NavigationManager(debug)
        self.action_learning_manager = ActionLearningManager(self.robot_controller)
        self.response_validator = ResponseValidator(self)
        
        # 向后兼容性属性
        self.calibration_distance_cm = calibration_distance_cm
        self.calibration_steps = calibration_steps
        self.step_distance_cm = self.config_manager.step_distance_cm
        self.step_distance_m = self.config_manager.step_distance_m
        self.speed_calibration = self.config_manager.speed_calibration
        self.default_config = self.config_manager.default_config
    
    # ==================== 配置管理接口 Configuration Management Interface ====================
    
    def update_config(self, **kwargs) -> tuple:
        """更新全局配置 Update global configuration"""
        return self.config_manager.update_config(**kwargs)
    
    def get_config(self) -> tuple:
        """获取配置 Get configuration"""
        return self.config_manager.get_config()
    
    def update_calibration(self, calibration_distance_cm: float, calibration_steps: int) -> tuple:
        """更新校准参数 Update calibration parameters"""
        result = self.config_manager.update_calibration(calibration_distance_cm, calibration_steps)
        # 更新向后兼容性属性
        self.calibration_distance_cm = calibration_distance_cm
        self.calibration_steps = calibration_steps
        self.step_distance_m = self.config_manager.step_distance_m
        return result
    
    def update_speed_calibration(self, calibration_data: Dict[int, float]) -> tuple:
        """更新速度标定数据 Update speed calibration data"""
        result = self.config_manager.update_speed_calibration(calibration_data)
        self.speed_calibration = self.config_manager.speed_calibration
        return result
    
    def calibrate_speed_levels(self) -> tuple:
        """校准速度档位 Calibrate speed levels"""
        return self.config_manager.calibrate_speed_levels()
    
    # ==================== 基础移动接口 Basic Movement Interface ====================
    
    def forward(self, steps: Optional[int] = None, distance_m: Optional[float] = None,
                time_s: Optional[float] = None, speed_ms: Optional[float] = None) -> tuple:
        """前进动作 Forward movement"""
        return self.robot_controller.forward(steps, distance_m, time_s, speed_ms)
    
    def backward(self, steps: Optional[int] = None, distance_m: Optional[float] = None,
                 time_s: Optional[float] = None, speed_ms: Optional[float] = None) -> tuple:
        """后退动作 Backward movement"""
        return self.robot_controller.backward(steps, distance_m, time_s, speed_ms)
    
    def shift_left(self, steps: Optional[int] = None, distance_m: Optional[float] = None,
                   time_s: Optional[float] = None, speed_ms: Optional[float] = None) -> tuple:
        """左平移动作 Left shift movement"""
        return self.robot_controller.shift_left(steps, distance_m, time_s, speed_ms)
    
    def shift_right(self, steps: Optional[int] = None, distance_m: Optional[float] = None,
                    time_s: Optional[float] = None, speed_ms: Optional[float] = None) -> tuple:
        """右平移动作 Right shift movement"""
        return self.robot_controller.shift_right(steps, distance_m, time_s, speed_ms)
    
    def rotate(self, direction: Optional[str] = None, angle_deg: Optional[float] = None,
               time_s: Optional[float] = None, angular_speed_deg_s: Optional[float] = None) -> tuple:
        """旋转动作 Rotation movement"""
        return self.robot_controller.rotate(direction, angle_deg, time_s, angular_speed_deg_s)
    
    # ==================== 预设动作接口 Preset Action Interface ====================
    
    def adjust_height(self, level: Optional[int] = None) -> tuple:
        """调整身高 Adjust height"""
        return self.robot_controller.execute_preset_action('adjust_height', level=level)
    
    def head_move(self, level: Optional[int] = None, direction: Optional[str] = None) -> tuple:
        """头部移动 Head movement"""
        return self.robot_controller.execute_preset_action('head_move', level=level, direction=direction)
    
    def stretch(self) -> tuple:
        """伸展动作 Stretch action"""
        return self.robot_controller.execute_preset_action('stretch')
    
    def say_hello(self) -> tuple:
        """打招呼动作 Say hello action"""
        return self.robot_controller.execute_preset_action('say_hello')
    
    def fear_retreat(self) -> tuple:
        """恐惧后退动作 Fear retreat action"""
        return self.robot_controller.execute_preset_action('fear_retreat')
    
    def warm_up_squat(self) -> tuple:
        """热身蹲起动作 Warm up squat action"""
        return self.robot_controller.execute_preset_action('warm_up_squat')
    
    def spin_in_place(self) -> tuple:
        """原地旋转动作 Spin in place action"""
        return self.robot_controller.execute_preset_action('spin_in_place')
    
    def wave_no(self) -> tuple:
        """摆手说不动作 Wave no action"""
        return self.robot_controller.execute_preset_action('wave_no')
    
    def curl_up(self) -> tuple:
        """蜷缩动作 Curl up action"""
        return self.robot_controller.execute_preset_action('curl_up')
    
    def big_stride(self) -> tuple:
        """大步走动作 Big stride action"""
        return self.robot_controller.execute_preset_action('big_stride')
    
    def stop_action(self) -> tuple:
        """停止动作 Stop action"""
        return self.robot_controller.execute_preset_action('stop_action')
    
    # ==================== 机器人状态接口 Robot Status Interface ====================
    
    def stop(self) -> tuple:
        """停止机器人 Stop robot"""
        return self.robot_controller.stop()
    
    def get_status(self) -> tuple:
        """获取机器人状态 Get robot status"""
        return self.robot_controller.get_status()
    
    def print_status(self) -> tuple:
        """打印机器人状态 Print robot status"""
        return self.robot_controller.print_status()
    
    def get_firmware_version(self) -> tuple:
        """获取固件版本 Get firmware version"""
        return self.robot_controller.get_firmware_version()
    
    def get_battery_info(self, as_voltage: bool = True) -> tuple:
        """获取电池信息 Get battery information"""
        return self.robot_controller.get_battery_info(as_voltage)
    
    def get_servo_angles(self, servo_id: Optional[int] = None) -> tuple:
        """获取舵机角度 Get servo angles"""
        return self.robot_controller.get_servo_angles(servo_id)
    
    def get_leg_angles(self, leg_id: int) -> tuple:
        """获取腿部角度 Get leg angles"""
        return self.robot_controller.get_leg_angles(leg_id)
    
    def get_attitude(self) -> tuple:
        """获取姿态信息 Get attitude information"""
        return self.robot_controller.get_attitude()
    
    def get_imu_raw_data(self) -> tuple:
        """获取IMU原始数据 Get IMU raw data"""
        return self.robot_controller.get_imu_raw_data()
    
    def get_servo_offset(self, servo_id: int) -> tuple:
        """获取舵机偏移 Get servo offset"""
        return self.robot_controller.get_servo_offset(servo_id)
    
    def print_system_info(self) -> tuple:
        """打印系统信息 Print system information"""
        return self.sensor_manager.print_system_info()
    
    # ==================== 传感器管理接口 Sensor Management Interface ====================
    
    def check_ros2_node_running(self, node_name: str, namespace: str = "/") -> tuple:
        """检查ROS2节点运行状态 Check ROS2 node running status"""
        return self.sensor_manager.check_ros2_node_running(node_name, namespace)
    
    def check_ros2_topic_active(self, topic_name: str, expected_type: str = "sensor_msgs/msg/Image") -> tuple:
        """检查ROS2话题活跃状态 Check ROS2 topic active status"""
        return self.sensor_manager.check_ros2_topic_active(topic_name, expected_type)
    
    def start_camera(self, check_before_start: bool = True, wait_time_s: float = 5.0) -> tuple:
        """启动相机 Start camera"""
        return self.sensor_manager.start_camera(check_before_start, wait_time_s)
    
    def get_camera_status(self) -> tuple:
        """获取相机状态 Get camera status"""
        return self.sensor_manager.get_camera_status()
    
    def check_camera_status(self) -> tuple:
        """检查相机状态 Check camera status"""
        return self.sensor_manager.check_camera_status()
    
    def stop_camera(self, force_kill: bool = False, wait_time_s: float = 3.0) -> tuple:
        """停止相机 Stop camera"""
        return self.sensor_manager.stop_camera(force_kill, wait_time_s)
    
    def capture_image(self, save_path: str = None, timeout_s: float = 10.0, 
                     check_camera_first: bool = True, image_format: str = "jpg") -> tuple:
        """拍摄图像 Capture image"""
        return self.sensor_manager.capture_image(save_path, timeout_s, check_camera_first, image_format)
    
    def get_depth_at_point(self, x: int, y: int, timeout_s: float = 10.0) -> tuple:
        """获取指定点的深度信息 Get depth information at specified point"""
        return self.sensor_manager.get_depth_at_point(x, y, timeout_s)
    
    def start_lidar(self, wait_time_s: float = 10.0, check_before_start: bool = True) -> tuple:
        """启动雷达 Start lidar"""
        return self.sensor_manager.start_lidar(wait_time_s, check_before_start)
    
    def get_lidar_data(self, timeout_s: float = 10.0, sample_count: int = 1) -> tuple:
        """获取雷达数据 Get lidar data"""
        return self.sensor_manager.get_lidar_data(timeout_s, sample_count)
    
    def _get_lidar_type(self) -> str:
        """
        获取当前雷达类型
        Get current lidar type from environment variable
        
        Returns:
            str: 雷达类型 ('a1' 或 '4ROS') Lidar type ('a1' or '4ROS')
        """
        return self.sensor_manager._get_lidar_type()
    
    def check_lidar_status(self) -> tuple:
        """检查雷达状态 Check lidar status"""
        return self.sensor_manager.check_lidar_status()
    
    def stop_lidar(self, force_kill: bool = False, wait_time_s: float = 3.0) -> tuple:
        """停止雷达 Stop lidar"""
        return self.sensor_manager.stop_lidar(force_kill, wait_time_s)
    
    def get_robot_position_in_map(self, timeout_s: float = 10.0) -> tuple:
        """获取机器人在地图中的位置 Get robot position in map"""
        return self.sensor_manager.get_robot_position_in_map(timeout_s)
    
    # ==================== 位置管理接口 Position Management Interface ====================
    
    def save_current_position(self, position_name: str, timeout_s: float = 10.0) -> tuple:
        """保存当前位置 Save current position"""
        return self.position_manager.save_current_position(position_name, timeout_s)
    
    def list_saved_positions(self) -> tuple:
        """列出已保存的位置 List saved positions"""
        return self.position_manager.list_saved_positions()
    
    def get_saved_position(self, position_name: str) -> tuple:
        """获取已保存的位置信息 Get saved position information"""
        return self.position_manager.get_saved_position(position_name)
    
    def delete_saved_position(self, position_name: str) -> tuple:
        """删除已保存的位置 Delete saved position"""
        return self.position_manager.delete_saved_position(position_name)
    
    # ==================== 导航管理接口 Navigation Management Interface ====================
    
    def check_navigation_node_status(self, node_name: str) -> tuple:
        """检查导航节点状态 Check navigation node status"""
        return self.navigation_manager.check_node_status(node_name)
    
    def check_navigation_system_status(self) -> tuple:
        """检查导航系统整体状态 Check navigation system overall status"""
        return self.navigation_manager.check_navigation_system_status()
    
    def set_initial_pose(self, x: float, y: float, yaw: float, timeout_s: float = 10.0) -> tuple:
        """设置机器人初始姿态 Set robot initial pose"""
        return self.navigation_manager.set_initial_pose(x, y, yaw)
    
    def navigate_to_pose(self, x: float, y: float, yaw: float, timeout_s: float = 60.0) -> tuple:
        """导航到指定位姿 Navigate to specified pose"""
        return self.navigation_manager.navigate_to_pose(x, y, yaw, timeout=timeout_s)
    
    def cancel_navigation(self) -> tuple:
        """取消当前导航任务 Cancel current navigation task"""
        return self.navigation_manager.cancel_navigation()
    
    def start_navigation_system(self, wait_time_s: float = 15.0) -> tuple:
        """启动导航系统 Start navigation system"""
        return self.navigation_manager.start_navigation_system(wait_time_s=wait_time_s)
    
    def start_laser_bringup(self, wait_time_s: float = 10.0) -> tuple:
        """启动激光雷达组件 Start laser bringup"""
        return self.navigation_manager.start_laser_bringup(wait_time_s)
    
    def start_laser_odometry(self, wait_time_s: float = 5.0) -> tuple:
        """启动激光雷达里程计 Start laser odometry"""
        return self.navigation_manager.start_laser_odometry(wait_time_s)
    
    def stop_laser_bringup(self, force_kill: bool = False, wait_time_s: float = 3.0) -> tuple:
        """停止激光雷达组件 Stop laser bringup"""
        return self.navigation_manager.stop_laser_bringup(force_kill, wait_time_s)
    
    def stop_laser_odometry(self, force_kill: bool = False, wait_time_s: float = 3.0) -> tuple:
        """停止激光雷达里程计 Stop laser odometry"""
        return self.navigation_manager.stop_laser_odometry(force_kill, wait_time_s)
    
    def stop_navigation_system(self, force_kill: bool = False, wait_time_s: float = 5.0) -> tuple:
        """停止导航系统 Stop navigation system"""
        return self.navigation_manager.stop_navigation_system(include_laser_components=True, force_kill=force_kill, wait_time_s=wait_time_s)
    
    # ==================== 向后兼容性方法 Backward Compatibility Methods ====================
    
    def _get_calibration_data(self) -> Dict[str, Any]:
        """获取校准数据 Get calibration data (backward compatibility)"""
        return self.config_manager._get_calibration_data()
    
    def _calculate_steps_from_distance(self, distance_m: float) -> tuple:
        """根据距离计算步数 Calculate steps from distance (backward compatibility)"""
        return self.config_manager._calculate_steps_from_distance(distance_m)
    
    def _calculate_step_interval_from_speed(self, speed_ms: float) -> tuple:
        """根据速度计算步间隔 Calculate step interval from speed (backward compatibility)"""
        return self.config_manager._calculate_step_interval_from_speed(speed_ms)
    
    def _select_optimal_speed_level(self, target_speed_ms: float) -> tuple:
        """选择最优速度档位 Select optimal speed level (backward compatibility)"""
        return self.config_manager._select_optimal_speed_level(target_speed_ms)
    
    # ==================== 返回值校验方法 Response Validation Methods ====================
    
    def validate_all_responses(self) -> tuple:
        """验证所有接口方法的返回值格式 Validate return value format of all interface methods"""
        return self.response_validator.validate_all_responses()
    
    @ensure_response_format
    def test_response_format_decorator(self, test_value: Any = None) -> Any:
        """测试返回值格式装饰器 Test response format decorator"""
        return self.response_validator.test_response_format_decorator(test_value)
    
    # ==================== 动作学习接口 Action Learning Interface ====================
    
    def prepare_learning_posture(self) -> tuple:
        """进入准备学习的姿态 Enter the posture ready to learn"""
        try:
            success = self.action_learning_manager.prepare_learning_posture()
            return (True, "学习准备姿态设置完成" if success else "设置学习姿态失败")
        except Exception as e:
            return (False, f"设置学习姿态异常: {str(e)}")
    
    def start_action_learning(self) -> tuple:
        """开始动作学习模式 Start action learning mode"""
        try:
            success = self.action_learning_manager.start_learning()
            return (True, "动作学习模式已启动" if success else "启动学习模式失败")
        except Exception as e:
            return (False, f"启动学习模式异常: {str(e)}")
    
    def stop_action_learning(self) -> tuple:
        """停止动作学习模式 Stop action learning mode"""
        try:
            success = self.action_learning_manager.stop_learning()
            return (True, "动作学习模式已停止" if success else "停止学习模式失败")
        except Exception as e:
            return (False, f"停止学习模式异常: {str(e)}")
    
    def record_current_action(self) -> tuple:
        """记录当前动作 Record current action"""
        try:
            success = self.action_learning_manager.record_current_action()
            sequence_length = len(self.action_learning_manager.current_action_sequence)
            if success:
                return (True, f"动作已记录，当前序列长度: {sequence_length}")
            else:
                return (False, "记录动作失败")
        except Exception as e:
            return (False, f"记录动作异常: {str(e)}")
    
    def save_action_sequence(self, filename: str) -> tuple:
        """保存当前动作序列到文件 Save current action sequence to file"""
        try:
            success = self.action_learning_manager.save_action_sequence(filename)
            return (True, f"动作序列已保存到: {filename}.json" if success else "保存动作序列失败")
        except Exception as e:
            return (False, f"保存动作序列异常: {str(e)}")
    
    def load_action_sequence(self, filename: str) -> tuple:
        """从文件加载动作序列 Load action sequence from file"""
        try:
            actions = self.action_learning_manager.load_action_sequence(filename)
            if actions is not None:
                return (True, {"actions": actions, "count": len(actions)})
            else:
                return (False, "加载动作序列失败")
        except Exception as e:
            return (False, f"加载动作序列异常: {str(e)}")
    
    def play_action_sequence(self, actions: Optional[list] = None, filename: Optional[str] = None, 
                           speed: int = 800, delay: float = 1.0) -> tuple:
        """播放动作序列 Play action sequence"""
        try:
            # 如果提供了文件名，从文件加载动作序列
            if filename and not actions:
                actions = self.action_learning_manager.load_action_sequence(filename)
                if not actions:
                    return (False, f"无法从文件 {filename} 加载动作序列")
            
            # 如果没有提供动作序列，使用当前序列
            if not actions:
                actions = self.action_learning_manager.current_action_sequence
            
            success = self.action_learning_manager.play_action_sequence(actions, speed, delay)
            return (True, f"动作序列播放完成，共 {len(actions)} 个动作" if success else "播放动作序列失败")
        except Exception as e:
            return (False, f"播放动作序列异常: {str(e)}")
    
    def clear_current_action_sequence(self) -> tuple:
        """清空当前动作序列 Clear current action sequence"""
        try:
            success = self.action_learning_manager.clear_current_sequence()
            return (True, "当前动作序列已清空" if success else "清空动作序列失败")
        except Exception as e:
            return (False, f"清空动作序列异常: {str(e)}")
    
    def delete_action_file(self, filename: str) -> tuple:
        """删除动作文件 Delete action file"""
        try:
            success = self.action_learning_manager.delete_action_file(filename)
            return (True, f"文件 {filename}.json 已删除" if success else "删除文件失败")
        except Exception as e:
            return (False, f"删除文件异常: {str(e)}")
    
    def list_action_files(self) -> tuple:
        """列出所有动作文件 List all action files"""
        try:
            files = self.action_learning_manager.list_action_files()
            return (True, {"files": files, "count": len(files)})
        except Exception as e:
            return (False, f"列出文件异常: {str(e)}")
    
    def get_action_learning_status(self) -> tuple:
        """获取动作学习状态 Get action learning status"""
        try:
            status = self.action_learning_manager.get_learning_status()
            return (True, status)
        except Exception as e:
            return (False, f"获取学习状态异常: {str(e)}")
    
    def print_action_learning_status(self) -> tuple:
        """打印动作学习状态 Print action learning status"""
        try:
            self.action_learning_manager.print_status()
            return (True, "状态信息已打印")
        except Exception as e:
            return (False, f"打印状态异常: {str(e)}")